#pragma once

#include <gazebo/common/Plugin.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/gazebo.hh>
#include <rclcpp/rclcpp.hpp>
#include <string>
#include <functional>

namespace gazebo
{
class MimicJointPlugin : public ModelPlugin
{
public:
  MimicJointPlugin() = default;
  virtual ~MimicJointPlugin() = default;

  /// \brief Load plugin
  void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) override;

private:
  /// \brief Called every simulation iteration
  void OnUpdate();

  // Gazebo joints
  physics::JointPtr joint_;   // 被驱动的关节
  physics::JointPtr mimic_;   // 主关节

  // Mimic 参数
  double multiplier_{1.0};
  double offset_{0.0};

  // ROS node
  rclcpp::Node::SharedPtr node_;

  // 更新连接
  event::ConnectionPtr updateConn_;
};
} // namespace gazebo
